"""

Path tracking simulation with iterative linear model predictive control for speed and steer control

author: Atsushi Sakai (@Atsushi_twi)

"""
import matplotlib.pyplot as plt
import time
import cvxpy
import math
import numpy as np
import sys
import os
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
"""
1 pathlib.Path(__file__) → C:/project/src/module/script.py
2 .parent.parent.parent → C:/project
3 str(...) → 'C:/project'
4 sys.path.append(...) → 将 'C:/project' 添加到 sys.path 中。
5 当您尝试导入一个模块时,Python 会按照 sys.path 中列出的目录顺序进行搜索
"""

from utils.angle import angle_mod # 是将角度值限制在 -π 到 π 之间

# 导入三次样条路径规划器
from PathPlanning.CubicSpline import cubic_spline_planner

NX = 4  # x = x, y, v, yaw   # 状态变量的数量
NU = 2  # a = [accel, steer] # 控制变量的数量
T = 5   # horizon length     # 预测时域

# mpc parameters MPC 权重矩阵
R = np.diag([0.01, 0.01])           # input cost matrix
Rd = np.diag([0.01, 1.0])           # input difference cost matrix
Q = np.diag([1.0, 1.0, 0.5, 0.5])   # state cost matrix
Qf = Q                  # state final matrix
GOAL_DIS = 1.5          # goal distance
STOP_SPEED = 0.5 / 3.6  # stop speed
MAX_TIME = 500.0        # max simulation time 最大迭代次数

# iterative paramter
MAX_ITER = 3  # Max iteration
DU_TH = 0.1  # iteration finish param

TARGET_SPEED = 8.0 / 3.6 # [m/s] target speed 目标速度10km/h
N_IND_SEARCH = 10         # Search index number

DT = 0.2  # [s] time tick

# Vehicle parameters 车辆参数
LENGTH = 4.5        # [m] 车辆长度
WIDTH = 2.0         # [m] 车辆宽度
BACKTOWHEEL = 1.0   # [m] 
WHEEL_LEN = 0.3     # [m]
WHEEL_WIDTH = 0.2   # [m]
TREAD = 0.7         # [m]
WB = 2.5            # [m]

# 约束条件
MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad]
MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s]
MAX_SPEED = 55.0 / 3.6        # maximum speed [m/s]
MIN_SPEED = -20.0 / 3.6       # minimum speed [m/s]
MAX_ACCEL = 1.0               # maximum accel [m/ss]

show_animation = True

# 车辆状态类
class State:
    """
    vehicle state class
    """

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v      
        self.predelta = None

# 将角度限制在[-pi, pi]的角度范围内
def pi_2_pi(angle):
    return angle_mod(angle)

# 生成线性化模型矩阵 A、B 和 C: x_{k+1} = A*x_{k} + B*u_{k} + C*w_{k}
def get_linear_model_matrix(v, phi, delta):
    """
    input:
        - v: 车辆当前的速度;
        - phi: 车辆当前的偏航角;
        - delta: 车辆当前的转向角;
    output:
        - A, B, C:线性化模型的矩阵 A、B 和向量 C
    """
    A = np.zeros((NX, NX))
    A[0, 0] = 1.0
    A[1, 1] = 1.0
    A[2, 2] = 1.0
    A[3, 3] = 1.0
    A[0, 2] = DT * math.cos(phi)
    A[0, 3] = - DT * v * math.sin(phi)
    A[1, 2] = DT * math.sin(phi)
    A[1, 3] = DT * v * math.cos(phi)
    A[3, 2] = DT * math.tan(delta) / WB

    B = np.zeros((NX, NU))
    B[2, 0] = DT
    B[3, 1] = DT * v / (WB * math.cos(delta) ** 2)

    C = np.zeros(NX)
    C[0] = DT * v * math.sin(phi) * phi
    C[1] = - DT * v * math.cos(phi) * phi
    C[3] = - DT * v * delta / (WB * math.cos(delta) ** 2)

    return A, B, C

# plot_car函数用于绘制车辆的位置和方向，看起来是图形化显示车辆状态的
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):  # pragma: no cover

    outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
                        [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])

    fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
                         [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])

    rr_wheel = np.copy(fr_wheel)

    fl_wheel = np.copy(fr_wheel)
    fl_wheel[1, :] *= -1
    rl_wheel = np.copy(rr_wheel)
    rl_wheel[1, :] *= -1

    Rot1 = np.array([[math.cos(yaw), math.sin(yaw)],
                     [-math.sin(yaw), math.cos(yaw)]])
    Rot2 = np.array([[math.cos(steer), math.sin(steer)],
                     [-math.sin(steer), math.cos(steer)]])

    fr_wheel = (fr_wheel.T.dot(Rot2)).T
    fl_wheel = (fl_wheel.T.dot(Rot2)).T
    fr_wheel[0, :] += WB
    fl_wheel[0, :] += WB

    fr_wheel = (fr_wheel.T.dot(Rot1)).T
    fl_wheel = (fl_wheel.T.dot(Rot1)).T

    outline = (outline.T.dot(Rot1)).T
    rr_wheel = (rr_wheel.T.dot(Rot1)).T
    rl_wheel = (rl_wheel.T.dot(Rot1)).T

    outline[0, :] += x
    outline[1, :] += y
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y

    plt.plot(np.array(outline[0, :]).flatten(),
             np.array(outline[1, :]).flatten(), truckcolor)
    plt.plot(np.array(fr_wheel[0, :]).flatten(),
             np.array(fr_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(rr_wheel[0, :]).flatten(),
             np.array(rr_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(fl_wheel[0, :]).flatten(),
             np.array(fl_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(rl_wheel[0, :]).flatten(),
             np.array(rl_wheel[1, :]).flatten(), truckcolor)
    plt.plot(x, y, "*")

# update_state函数根据控制输入（加速度和转向角）更新车辆状态，
# 这里应用了车辆的运动学模型，考虑了转向角对偏航角变化的影响，以及加速度对速度的影响。
def update_state(state, a, delta):

    # input check
    if delta >= MAX_STEER:
        delta = MAX_STEER
    elif delta <= -MAX_STEER:
        delta = -MAX_STEER

    state.x = state.x + state.v * math.cos(state.yaw) * DT
    state.y = state.y + state.v * math.sin(state.yaw) * DT
    state.yaw = state.yaw + state.v / WB * math.tan(delta) * DT
    state.v = state.v + a * DT

    if state.v > MAX_SPEED:
        state.v = MAX_SPEED
    elif state.v < MIN_SPEED:
        state.v = MIN_SPEED

    return state


def get_nparray_from_matrix(x):
    return np.array(x).flatten()

# 计算车辆当前位置在参考路径上的最近点的索引，用于跟踪参考轨迹
def calc_nearest_index(state, path_x, path_y, path_yaw, start_ind):
    """
    input param:
     - state: 车辆当前状态;
     - [path_x,path_y,path_yaw]: 路径序列;
     - start_ind: 局部路径段的起始点;

    output:
     - nearest_ind: 在局部路径段中,离当前状态最近的点;
     - dis: 当前状态离nearst_ind的距离;
    """

    dx = [state.x - icx for icx in path_x[start_ind:(start_ind + N_IND_SEARCH)]]
    dy = [state.y - icy for icy in path_y[start_ind:(start_ind + N_IND_SEARCH)]]

    d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]

    dis = min(d)

    nearest_ind = d.index(dis) + start_ind

    dis = math.sqrt(dis)

    dxl = path_x[nearest_ind] - state.x
    dyl = path_y[nearest_ind] - state.y

    angle = pi_2_pi(path_yaw[nearest_ind] - math.atan2(dyl, dxl))
    if angle < 0:
        dis *= -1

    return nearest_ind, dis

# 根据给定的控制输入序列预测车辆在未来时间段内的状态轨迹。
def predict_motion(x0, ua, usteer, local_path_ref):
    """
    input param:
     - x0: 车初始状态.形状为(NX,)的数组,表示车辆当前的状态，包括位置、速度和偏航角。
     - ua: 加速度控制输入.形状为(T,)的数组,表示在预测时间段内每个时间步的加速度控制输入。
     - usteer: 转向角控制输入.形状为(T,)的数组,表示在预测时间段内每个时间步的转向角控制输入。
     - local_path_ref: 参考轨迹.形状为(NX,T+1)的数组,表示车辆在预测时间段内期望达到的状态轨迹。
    output:
     - x_predict: 预测的状态轨迹.形状为(NX,T+1)的数组,表示车辆在预测时间段内的状态轨迹.包括位置,速度和偏航角。
     x_predict 存储的值如下:
     ---                                                          ---   
     | x0[0], state1[0], state2[0], state3[0], state4[0], state5[0] |
     | x0[1], state1[1], state2[1], staet3[1], state4[1], state5[1] |
     | x0[2], state1[2], state2[2], state3[2], state4[2], state5[2] |
     | x0[3], state1[3], state2[3], state3[3], state4[3], state5[3] |
     ---                                                          ---
    """

    # 初始化预测状态轨迹 x_predict
    # 创建一个形状为(NX,T+1)的数组用于存储预测的状态轨迹，
    # x_predict的初始值设置为参考轨迹local_path_ref的零值。
    x_predict = local_path_ref * 0.0 # [Nx,T+1]
    for i, _ in enumerate(x0):
        x_predict[i, 0] = x0[i]

    # 初始化车辆状态: 
    # 使用初始状态 x0 创建一个 State 对象，表示车辆的当前状态。
    state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2])

    # 预测状态轨迹
    for (ai, di, i) in zip(ua, usteer, range(1, T + 1)):
        # 根据当前的加速度 ua[i] 和转向角 usteer[i] 更新车辆状态
        state = update_state(state, ai, di)
        x_predict[0, i] = state.x
        x_predict[1, i] = state.y
        x_predict[2, i] = state.v
        x_predict[3, i] = state.yaw

    return x_predict


# 返回优化后的控制输入和状态轨迹，用于更新车辆的状态并继续跟踪路径。
def iterative_linear_mpc_control(local_path_ref, x0, steer_ref, oa, usteer):
    """
    作用：
        它的主要作用是通过迭代优化控制输入, 使车辆能够更准确地跟踪参考轨迹。
        通过多次调用线性 MPC 控制器，逐步优化控制输入，直到满足收敛条件
        (控制输入的变化小于阈值DU_TH)或达到最大迭代次数, 从而提高车辆的路径跟踪性能和稳定性。
    具体来说：
        它接收参考轨迹 local_path_ref 和车辆的初始状态 x0, 以及上一次的控制输入 ua 和 usteer。
        通过迭代优化，计算出新的控制输入 ua 和 usteer, 使得车辆在预测时间段内的状态轨迹更接近参考轨迹。
    input param:
     - local_path_ref: 局部参考轨迹。形状为 (NX,T+1) 的数组, 其中 NX 是状态变量的数量, T 是预测时间段。
     local_path_ref 包含了车辆在预测时间段内期望达到的状态, 包括位置、速度和偏航角。
     - x0: 车辆初始状态。形状为 (NX,1) 的数组, 表示车辆当前的状态，包括位置、速度和偏航角。
     - steer_ref: 转向角参考。形状为 (1,T+1) 的数组, 表示在预测时间段内车辆期望的转向角。
     - ua: 上一次的加速度控制输入。形状为 (T,) 的数组, 表示上一次迭代中优化得到的加速度控制输入。如果为 None, 则初始化为零。
     - usteer: 上一次的转向角控制输入。形状为 (T,) 的数组, 表示上一次迭代中优化得到的转向角控制输入。如果为 None, 则初始化为零。
    output:
     - ua: 优化后的加速度控制输入。形状为 (T,) 的数组，表示本次迭代中优化得到的加速度控制输入。
     - usteer: 优化后的转向角控制输入。形状为 (T,) 的数组，表示本次迭代中优化得到的转向角控制输入。
     - ox: 优化后的状态轨迹。形状为 (NX, T + 1) 的数组，表示车辆在预测时间段内的优化状态轨迹。
     - oy: 优化后的 y 坐标轨迹。形状为 (T + 1,) 的数组，表示车辆在预测时间段内的 y 坐标轨迹。
     - oyaw: 优化后的偏航角轨迹。形状为 (T + 1,) 的数组，表示车辆在预测时间段内的偏航角轨迹。
     - ov: 优化后的速度轨迹。形状为 (T + 1,) 的数组，表示车辆在预测时间段内的速度轨迹。
    """
    ox, oy, oyaw, ov = None, None, None, None

    # 初始化控制输入
    # 如果 ua 或 usteer 为 None，则将其初始化为零数组，表示初始的控制输入
    if ua is None or usteer is None:
        # T(5)组控制输入
        ua = [0.0] * T # ua size: [1x5]
        usteer = [0.0] * T # usteer size: [1x5]
    print(f"[Info] line {sys._getframe().f_lineno}:", f"pre_ua: {np.round(ua,4)}", f"; pre_usteer: {np.round(usteer,4)}")

    # 迭代优化：
    # 进行最多 MAX_ITER 次迭代，直到控制输入的变化小于阈值 DU_TH 或达到最大迭代次数
    for i in range(MAX_ITER):
        # 根据当前的控制输入 ua 和 usteer，预测车辆在未来时间段内的状态轨迹 x_predict(size:[NX,T+1])
        x_predict = predict_motion(x0, ua, usteer, local_path_ref)
        print(f"[Info] line {sys._getframe().f_lineno}:", f"*****iter: {i}",f"; predict traj x_predict")
        print(f"{np.round(x_predict)}")

        pua, pusteer = ua[:], usteer[:] # 记录为上一次的控制输入

        # 根据预测的状态轨迹 x_predict、参考轨迹 local_path_ref、初始状态 x0 和转向角参考 steer_ref，优化控制输入 ua 和 usteer
        ua, usteer, ox, oy, oyaw, ov = linear_mpc_control(local_path_ref, x_predict, x0, steer_ref)
        
        # 计算控制输入的变化量 du，如果变化量小于阈值 DU_TH，则停止迭代
        du = sum(abs(ua - pua)) + sum(abs(usteer - pusteer))  # calc u change value
        print(f"[Info] line {sys._getframe().f_lineno}:", f"; temp_ua: {np.round(ua, 4)}", f"; temp_usteer: {np.round(usteer, 4)}", f"; du: {du:.2f};")
        if du <= DU_TH: # DU_TH: 0.1
            break
        #else:
        #    print("Iterative is max iter")
        
    print(f"[Info] line {sys._getframe().f_lineno}:", f"result_ua: {np.round(ua, 4)}", f"; result_usteer: {np.round(usteer, 4)}")
    print(f"[Info] line {sys._getframe().f_lineno}:", f"result_ox: {np.round(ox, 4)}", f"; result_oy: {np.round(oy, 4)}", f"; result_oyaw: {np.round(oyaw, 4)}", f"; result_ov: {np.round(ov,4)}")
    
    return ua, usteer, ox, oy, oyaw, ov


# 多次求解MPC问题，每次用前一次的解作为操作点，更新线性化模型，以提高控制的准确性。
def linear_mpc_control(local_path_ref, x_predict, x0, ref_steer):
    """
    作用:
     linear mpc control
     函数构建了MPC的优化问题，定义了状态变量、控制变量、代价函数和约束条件,
     使用cvxpy进行求解。代价函数包括跟踪参考轨迹的误差、控制输入的代价以及控制输入变化的代价.
     约束条件包括车辆动力学模型、初始状态、速度限制、加速度和转向角限制等。这里使用了迭代的方法，

     linear_mpc_control 函数的核心作用就是：在满足各种约束条件下，调整状态变量 x 和控制变量 u, 
     使得代价函数 cost 最小化。
     
    input param:
     - local_path_ref: reference point 参考轨迹, 形状为 (NX, T+1) 的数组，其中 NX 是状态变量的数量, T 是预测时间段。
    local_path_ref 包含了车辆在预测时间段内期望达到的状态，包括位置、速度和偏航角。
     - x_predict: 提供线性化操作点。
     - x0: 车辆的初始状态。形状为 (NX,1) 的数组，表示车辆当前的状态，包括位置、速度和偏航角。
     - ref_steer: 参考转向角。形状为 (1, T+1) 的数组，表示在预测时间段内车辆期望的转向角。
    
    output:
     - ua_opt: 优化后的加速度控制输入。形状为 (T,1) 的数组，表示本次迭代中优化得到的加速度控制输入。
     - usteer_opt: 优化后的转向角控制输入。形状为 (T,1) 的数组，表示本次迭代中优化得到的转向角控制输入。
     - x_opt: 优化后的状态轨迹。形状为 (NX, T+1) 的数组，表示车辆在预测时间段内的优化状态轨迹。
     - y_opt: 优化后的 y 坐标轨迹。形状为 (T+1,1) 的数组，表示车辆在预测时间段内的 y 坐标轨迹。
     - yaw_opt: 优化后的偏航角轨迹。形状为 (T+1,) 的数组，表示车辆在预测时间段内的偏航角轨迹。
     - v_opt: 优化后的速度轨迹。形状为 (T+1,) 的数组，表示车辆在预测时间段内的速度轨迹。
    """

    # 定义变量
    x = cvxpy.Variable((NX, T+1))   # 状态变量x，维度为 (NX, T+1)，表示车辆在预测时间内的状态
    u = cvxpy.Variable((NU, T))     # 控制变量u，维度为 (NU, T)，表示车辆在预测时间内的控制输入

    cost = 0.0       # 代价函数
    constraints = [] # 约束条件

    # 循环优化: 在预测时间内的每一步，计算代价函数和约束条件
    for t in range(T):
        # 构建代价函数:
        # 对控制输入 u 施加二次代价，其中 R 是权重矩阵，用于平衡控制输入的大小
        cost += cvxpy.quad_form(u[:, t], R)

        # 对状态变量 x 施加二次代价，用于跟踪参考轨迹 local_path_ref,
        # 其中 Q 是权重矩阵, 来平衡状态变量与参考轨迹的误差
        if t != 0:
            cost += cvxpy.quad_form(local_path_ref[:, t] - x[:, t], Q)

        # 使用线性化模型x_{t+1} = A*x_{t} + B*u_{t} + C
        # A 是状态转移矩阵; B 是控制输入矩阵; C 是常数
        A, B, C = get_linear_model_matrix(x_predict[2, t], x_predict[3, t], ref_steer[0, t]) #参数[速度,偏航角,转向角]
        constraints += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C] # 状态转移约束

        # 控制输入变化代价
        if t < (T - 1):
            # 对控制输入的变化施加二次代价, 其中Rd是权重矩阵
            cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
            # 限制转向角的变化率
            constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= MAX_DSTEER * DT]

    # 终端代价: 在预测时间的终端，施加终端代价，用于确保车辆最终接近参考轨迹
    cost += cvxpy.quad_form(local_path_ref[:, T] - x[:, T], Qf)    

    # 其他约束
    constraints += [x[:, 0] == x0]        # 初始化状态约束: 车辆的初始状态必须等于x0
    constraints += [x[2, :] <= MAX_SPEED] # 速度最大约束
    constraints += [x[2, :] >= MIN_SPEED] # 速度最小约束
    constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL] # 加速度最大约束
    constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER] # 转向角最大约束

    # 优化问题求解: 定义优化问题并使用 cvxpy 求解。这里使用了 CLARABEL 求解器
    prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
    prob.solve(solver=cvxpy.CLARABEL, verbose=False)

    # 提取结果: 如果优化问题求解成功, 提取优化后的状态和控制输入
    if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
        x_opt = get_nparray_from_matrix(x.value[0, :])      # 优化后的x坐标轨迹.
        y_opt = get_nparray_from_matrix(x.value[1, :])      # 优化后的y坐标轨迹.
        v_opt = get_nparray_from_matrix(x.value[2, :])      # 优化后的速度轨迹.
        yaw_opt = get_nparray_from_matrix(x.value[3, :])    # 优化后的偏航轨迹.
        ua_opt = get_nparray_from_matrix(u.value[0, :])     # 优化后的加速度控制输入.
        usteer_opt = get_nparray_from_matrix(u.value[1, :]) # 优化后的转角.
    else: # 如果求解失败, 返回None
        print("Error: Cannot solve mpc..")
        ua_opt, usteer_opt, x_opt, y_opt, yaw_opt, v_opt = None, None, None, None, None, None

    return ua_opt, usteer_opt, x_opt, y_opt, yaw_opt, v_opt

# 函数根据当前状态在参考路径中搜索局部参考轨迹，用于MPC的跟踪。
# 。
# MPC 可以优化车辆的控制输入(如加速度和转向角)以尽可能地跟踪参考轨迹，从而实现精确的路径跟踪和速度控制。
def calc_ref_trajectory(state, path_cx, path_cy, path_cyaw, path_ck, path_sp, dl, start_ind):
    """
    说明:
        具体来说，该函数根据当前车辆状态和路径数据，获取预测时间段内的轨迹点，作为车辆控制器的跟踪目标.
    input param:
     - state: 车辆状态;
     - path_cx, path_cy, path_cyaw, path_ck, path_sp: 路径点信息(坐标,航向角,曲率,路径长度);
     - dl: 路径点间距;
     - pind: 最近路径点索引;
    output:
     - xref: 参考轨迹。形状为 (NX, T + 1) 的数组，其中 NX 是状态变量的数量(通常包括位置、速度和方向), 
     T 是预测时间段。这个参考轨迹包含了车辆在预测时间段内期望达到的状态。
     - ind: 路径索引。表示车辆在生成参考轨迹后的新位置，通常是一个整数，指示车辆在路径中的当前位置。
     - dref: 转向角参考。形状为 (1, T + 1) 的数组，表示在预测时间段内车辆期望的转向角。
    """
    xref = np.zeros((NX, T + 1))
    dref = np.zeros((1, T + 1))
    ncourse = len(path_cx) # 参考路径点的数量

    # 查找最接近的路径点(ind:最近路径点索引, _:为距离)
    nearest_ind, _ = calc_nearest_index(state, path_cx, path_cy, path_cyaw, start_ind)

    # 索引安全检查
    if start_ind >= nearest_ind:
        nearest_ind = start_ind

    # 根据最接近的路径点 nearest_ind 和路径数据 path_cx, path_cy, path_cyaw, path_sp，初始化参考轨迹 xref
    xref[0, 0] = path_cx[nearest_ind]
    xref[1, 0] = path_cy[nearest_ind]
    xref[2, 0] = path_sp[nearest_ind]
    xref[3, 0] = path_cyaw[nearest_ind]
    dref[0, 0] = 0.0  # steer operational point should be 0

    # 提取参考轨迹: 
    # 在预测时间段内，根据车辆的速度和时间增量 DT，计算车辆能够行驶的距离，
    # 并在路径上找到相应数量的点，生成参考轨迹 xref
    travel = 0.0
    for i in range(T + 1):
        travel += abs(state.v) * DT    # 计算行驶距离
        dind = int(round(travel / dl)) # 距离换算成路径段数量
        #print(f"[Info] line {sys._getframe().f_lineno}: travel: {travel:0.2f}; dind: {dind}")

        # 将路径点的位置、速度和偏航角信息填充到参考轨迹 xref 中
        if (nearest_ind + dind) < ncourse:
            xref[0, i] = path_cx[nearest_ind + dind]   # 位置
            xref[1, i] = path_cy[nearest_ind + dind]
            xref[2, i] = path_sp[nearest_ind + dind]   # 速度
            xref[3, i] = path_cyaw[nearest_ind + dind] # 偏航角
            dref[0, i] = 0.0
        else:
            xref[0, i] = path_cx[ncourse - 1]
            xref[1, i] = path_cy[ncourse - 1]
            xref[2, i] = path_sp[ncourse - 1]
            xref[3, i] = path_cyaw[ncourse - 1]
            dref[0, i] = 0.0

    return xref, nearest_ind, dref

# 函数检查是否到达目标点
def check_goal(state, goal, tind, nind):

    # check goal
    dx = state.x - goal[0]
    dy = state.y - goal[1]
    d = math.hypot(dx, dy)

    isgoal = (d <= GOAL_DIS)

    if abs(tind - nind) >= 5:
        isgoal = False

    isstop = (abs(state.v) <= STOP_SPEED)

    if isgoal and isstop:
        return True

    return False

# 执行整个仿真过程，包括状态更新、MPC求解、图形显示等
def do_simulation(ref_cx, ref_cy, ref_cyaw, ref_ck, ref_sp, dl, initial_state):
    """
    Simulation
    input param:
     - [ref_cx,ref_cy,ref_cyaw,ref_ck,ref_sp]: 路径数据列表. 包括路径点坐标(x,y),偏航角,曲率,速度;
     - dl: course tick [m] 路径点间距
     - initial_state: 车辆初始状态;

    output param:
     - 
    """

    # 终点坐标
    goal_pos = [ref_cx[-1], ref_cy[-1]] 

    state = initial_state # 初始化状态
    print(f"[Info] line {sys._getframe().f_lineno}:", f"inite state(x,y,v,yaw): {state.x:.2f} {state.y:.2f} {state.v:.2f} {state.yaw:.2f}")

    # initial yaw compensation 初始偏航补偿
    if state.yaw - ref_cyaw[0] >= math.pi:
        state.yaw -= math.pi * 2.0
    elif state.yaw - ref_cyaw[0] <= -math.pi:
        state.yaw += math.pi * 2.0

    # 提取t0时刻车辆的状态(坐标,航向,速度)
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    d = [0.0]
    a = [0.0]

    # 在参考路径中搜索离车辆起始位置最近的点序号和距离
    nearest_ind, _ = calc_nearest_index(state, ref_cx, ref_cy, ref_cyaw, 0) # [目标点, 距离]
    print(f"[Info] line {sys._getframe().f_lineno}: nearest_ind: {nearest_ind}, mind:{_}")

    # 初始化输入控制
    odelta, oa = None, None 

    # 平滑航向角
    ref_cyaw = smooth_yaw(ref_cyaw)

    while MAX_TIME >= time:
        print(f"[Info] line {sys._getframe().f_lineno}: time: {time:.2f} ==================================================================")

        # 输入车辆状态, 路径信息列表, 目标路径点序号提取预测时间段内的局部轨迹点信息
        local_path_ref, nearest_ind, steer_ref = calc_ref_trajectory(state, ref_cx, ref_cy, ref_cyaw, ref_ck, ref_sp, dl, nearest_ind)
        print(f"[Info] line {sys._getframe().f_lineno}: cur_ind:{nearest_ind}; steer_ref:{steer_ref}; local_path_ref:; ")
        print(f"{np.round(local_path_ref,4)}")

        # 提取车辆当前状态
        x0 = [state.x, state.y, state.v, state.yaw]  # current state (x,y,v,yaw)
        print(f"[Info] line {sys._getframe().f_lineno}:", f"cur_state(x,y,v,yaw): {state.x:.2f} {state.y:.2f} {state.v:.2f} {state.yaw:.2f}")

        # 通过多次调用线性MPC控制器,逐步优化控制输入,直到满足收敛条件或达到最大迭代次数
        oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control(local_path_ref, x0, steer_ref, oa, odelta)

        # 提取第一个控制指令，并更新车辆状态
        di, ai = 0.0, 0.0
        if odelta is not None:
            di, ai = odelta[0], oa[0] # 提取第一个控制指令
            state = update_state(state, ai, di) # 更新车辆状态
        print(f"[Info] line {sys._getframe().f_lineno}:", f"update_state(x,y,v,yaw): {state.x:.2f} {state.y:.2f} {state.v:.2f} {state.yaw:.2f}")


        time = time + DT # 更新时间

        # 添加轨迹
        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)
        d.append(di)
        a.append(ai)

        # 检测是否到达目标点
        if check_goal(state, goal_pos, nearest_ind, len(ref_cx)):
            print("Goal")
            break

        # 展示动画
        if show_animation:  # pragma: no cover
            # 清除当前图形
            plt.cla() # 清除当前图形
            # 事件处理器
            # for stopping simulation with the esc key. 为图形界面的画布绑定一个按键事件监听器
            plt.gcf().canvas.mpl_connect('key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
            # 绘制路径
            plt.plot(local_path_ref[0, :], local_path_ref[1, :], "xk", label="local_path_ref") # local_path_ref: 绘制为黑色的"x"标记, 表示参考轨迹;
            if ox is not None:
                plt.plot(ox, oy, "xr", label="MPC") # ox和oy: 绘制为红色的"x"标记, 表示MPC轨迹
            plt.plot(ref_cx, ref_cy, "-r", label="course")  # ref_cx和ref_cy: 绘制为红色的实线, 表示路径;
            plt.plot(x, y, "ob", label="trajectory")# x和y: 绘制为蓝色的圆圈,表示车辆的历史轨迹;
            plt.plot(ref_cx[nearest_ind], ref_cy[nearest_ind], "xg", label="target") # 绘制为绿色的"x"标记, 表示目标点;
            
            # 绘制车辆的当前位置和方向(车辆坐标、车辆的偏航角、车辆的转向角)
            plot_car(state.x, state.y, state.yaw, steer=di) 
            
            # 设置图形属性
            plt.axis("equal") # 设置相同比例: 确保x/y轴的单位长度相同
            plt.grid(True) # 绘制网格: 显示网格线, 方便观察路径和轨迹
            # 设置标题: 显示当前的时间（以秒为单位）和车辆的速度（以 km/h 为单位）
            plt.title("Time[s]:" + str(round(time, 2)) + ", speed[m/s]:" + str(round(state.v, 2)))
            # 暂停: 暂停一小段时间，使画面有时间更新
            plt.pause(0.0001)

       # user_input = input("Press ENTER to continue, q to quit:")

    return t, x, y, yaw, v, d, a

# 这段代码通过计算路径点之间的方向变化和偏航角差异，动态调整速度，
# 以确保车辆在路径上行驶时能够正确地前进或后退
def calc_speed_profile(cx, cy, cyaw, target_speed):
    """
    input: 
     - cx: 路径点的 x 坐标。
     - cy: 路径点的 y 坐标。
     - cyaw: 路径点的偏航角（单位：弧度）。
     - target_speed: 期望的目标速度（单位：米/秒）。
    output:
     - speed_profile: 路径点的速度，其中每个元素是一个浮点数，表示该点的速度。正值表示前进，负值表示后退，零表示停止。
    """

    # 初始化路径点速度
    speed_profile = [target_speed] * len(cx)
    direction = 1.0  # forward 默认方向为前进

    # Set stop point 确认停止点并调整方向
    for i in range(len(cx) - 1):
        # 计算相邻点之间的方向变化
        dx = cx[i + 1] - cx[i]
        dy = cy[i + 1] - cy[i]
        # 是相邻点之间的移动方向（以弧度表示）
        move_direction = math.atan2(dy, dx)

        if dx != 0.0 and dy != 0.0:
            # 计算当前路径点的偏航角与移动方向之间的差异
            dangle = abs(pi_2_pi(move_direction - cyaw[i]))
            # 如果差值大于或等于 π/4（45度），则设置方向为后退（direction = -1.0），否则保持前进（direction = 1.0）
            if dangle >= math.pi / 4.0:
                direction = -1.0
            else:
                direction = 1.0

        # 根据方向变量 direction，调整速度轮廓
        if direction != 1.0:
            speed_profile[i] = - target_speed # 如果方向为后退（direction = -1.0），速度被设置为 -target_speed
        else:
            speed_profile[i] = target_speed # 如果方向为前进（direction = 1.0），速度保持为 target_speed

    speed_profile[-1] = 0.0 # 路径的最后一个点的速度被设置为零，表示车辆在路径终点停止
    #print("speed profile: ", [f"{num:.2f}" for num in speed_profile])

    return speed_profile

def calc_speed_profile_v1(cx, cy, cyaw, ck, target_speed):
    """
    Calculate the speed profile for the vehicle based on the path curvature and vehicle dynamics.

    input:
        - cx: 路径点的 x 坐标列表。
        - cy: 路径点的 y 坐标列表。
        - cyaw: 路径点的偏航角列表（单位：弧度）。
        - target_speed: 目标速度（单位：米/秒）。
        - ck: 路径点的曲率列表。

    output:
        - speed_profile: 路径点的速度列表，其中每个元素是一个浮点数，表示该点的速度。
    """
    # 最高速度和最低速度
    MAX_SPEED = target_speed
    MIN_SPEED = 1.0/3.6

    # 初始化速度轮廓
    speed_profile = [MAX_SPEED] * len(cx)
    direction = 1.0 # 默认为前进方向

    # 加速度和减速度
    ACC = 0.5  # 加速度 (m/s^2)
    DEC = 0.5  # 减速度 (m/s^2)

    # 计算速度轮廓
    for i in range(len(cx) - 1):
        # 计算相邻点之间的方向变化
        dx = cx[i + 1] - cx[i]
        dy = cy[i + 1] - cy[i]
        # 是相邻点之间的移动方向（以弧度表示）
        move_direction = math.atan2(dy, dx)

        if dx != 0.0 and dy != 0.0:
            # 计算当前路径点的偏航角与移动方向之间的差异
            dangle = abs(pi_2_pi(move_direction - cyaw[i]))
            # 如果差值大于或等于 π/4（45度），则设置方向为后退（direction = -1.0），否则保持前进（direction = 1.0）
            if dangle >= math.pi / 4.0:
                direction = -1.0
            else:
                direction = 1.0

        
        curvature = abs(ck[i]) # 路径点曲率取绝对值

        # 速度与曲率的关系
        # 速度与曲率负相关，曲率越大，速度越小
        # 使用一个简单的线性关系：v = MAX_SPEED - curvature * vehicle_length
        # 保证速度不低于最低速度
        speed = MAX_SPEED - curvature * LENGTH
        speed = max(speed, MIN_SPEED)
        # print("(cur, spe)", [f"{num:.2f}" for num in [curvature, speed]])

        """
        # 速度梯度变化
        # 在启动和停止时，速度有梯度变化的过程
        if (i == 0) or (i == n_course - 1):
            # 起点和终点速度为0.0
            speed = 0.0
        else:
            # 使用加速度和减速度计算速度变化
            # 假设每个路径点的时间间隔为DT
            DT = 0.1  # 时间间隔 (s)
            prev_speed = speed_profile[i-1]

            # 如果当前速度大于前一个速度，应用加速度
            if speed > prev_speed:
                speed = prev_speed + ACC * DT
            # 如果当前速度小于前一个速度，应用减速度
            elif speed < prev_speed:
                speed = prev_speed - DEC * DT
            else:
                speed = prev_speed
        """

        # 确保速度在允许的范围之类
        speed_profile[i] = speed * direction
        # speed_profile[i] = target_speed * direction

    # 打印速度轮廓
    speed_profile[-1] = 0.0 
    print("speed profile: ", [f"{num*3.6:.2f}" for num in speed_profile])

    return speed_profile

def calc_speed_profile_v2(cx, cy, cyaw, ck, target_speed):
    """
    Improved speed profile considering curvature, acceleration/deceleration ramps, and vehicle dynamics.
    """
    n = len(cx)
    speed_profile = np.full(n, target_speed)
    if n == 0:
        return speed_profile.tolist()

    # 1. 方向判断（保留原有逻辑）
    direction = np.ones(n) # 默认为前进方向
    for i in range(n-1):
        dx = cx[i+1] - cx[i]
        dy = cy[i+1] - cy[i]
        move_dir = math.atan2(dy, dx)
        d_angle = abs(pi_2_pi(move_dir - cyaw[i]))
        if d_angle >= math.pi / 4.0:
            direction[i] = -1.0
    direction[-1] = direction[-2] if n > 1 else 1.0 # 最后一个点的方向取前一个点的方向

    # 2. 动力学参数
    MAX_LATERAL_ACCEL = 2.0  # 最大横向加速度 (m/s²)
    WB = 2.5  # 轴距 (m)
    MAX_STEER = np.deg2rad(45.0)  # 最大转向角
    MIN_TURN_RADIUS = WB / np.tan(MAX_STEER)  # 最小转弯半径
    MAX_CURVATURE = 1.0 / MIN_TURN_RADIUS if MIN_TURN_RADIUS != 0 else np.inf

    # 3. 基于曲率的速度计算
    for i in range(n):
        # 曲率安全限幅
        safe_curvature = min(abs(ck[i]), MAX_CURVATURE)
        
        # 横向加速度约束速度计算
        if safe_curvature < 1e-5:
            v_curve = target_speed
        else:
            v_curve = np.sqrt(MAX_LATERAL_ACCEL / safe_curvature)
            v_curve = min(v_curve, target_speed)
        
        speed_profile[i] = v_curve
    
    # 6. 终点强制置零
    speed_profile[-1] = 0.0

    # 4. 启动/停止渐变处理（三角函数平滑）
    ramp_ratio = 0.05  # 渐变区占总路径的5%
    n_ramp = max(10, int(n * ramp_ratio)) # 至少10个点
    
    # 启动时速度渐变
    #for i in range(n_ramp):
    #    ratio = 0.5 * (1 - np.cos(np.pi * i / n_ramp)) #  ratio = [0, 1]
    #    speed_profile[i] *= ratio
        
    # 停止时速度渐变
    #for i in range(max(0, n-n_ramp), n):
    #    ratio = 0.5 * (1 + np.cos(np.pi * (i - (n-n_ramp)) / n_ramp)) # ratio = [1, 0]之间
    #    speed_profile[i] *= ratio

    # 5. 应用方向并限制速度范围
    speed_profile = np.clip(speed_profile * direction, -target_speed, target_speed)
    
    # 7. 速度平滑滤波（移动平均）
    window_size = 5
    if n > window_size:
        kernel = np.ones(window_size) / window_size
        speed_profile = np.convolve(speed_profile, kernel, mode='same')

    print("speed profile: ", [f"{num*3.6:.2f}" for num in speed_profile.tolist()])
    # print("[DEBUG] Speed profile:", np.round(speed_profile, 2))

    return speed_profile.tolist()


# 函数 smooth_yaw(yaw) 的作用是对 yaw 列表中的偏航角进行平滑处理。
# 具体来说，它确保连续的偏航角之间的差值在 -π/2 到 π/2 弧度范围内。
# 这很重要，因为偏航角可能会从 π 跳到 -π 或者从 -π 跳到 π，从而导致角度值出现不连续的情况。
def smooth_yaw(yaw):
    """
    input param:
     - yaw: 偏航角列表;
    output:
     - yaw: 平滑后的偏航角;
    """

    for i in range(len(yaw) - 1):
        dyaw = yaw[i + 1] - yaw[i]

        while dyaw >= math.pi / 2.0:
            yaw[i + 1] -= math.pi * 2.0
            dyaw = yaw[i + 1] - yaw[i]

        while dyaw <= -math.pi / 2.0:
            yaw[i + 1] += math.pi * 2.0
            dyaw = yaw[i + 1] - yaw[i]

    return yaw

# get_straight_course生成直线路径
def get_straight_course(dl):
    ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
    ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=dl)

    return cx, cy, cyaw, ck

def get_straight_course2(dl):
    ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
    ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=dl)

    return cx, cy, cyaw, ck

# get_forward_course生成曲线路径
def get_straight_course3(dl):
    ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
    ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=dl)

    cyaw = [i - math.pi for i in cyaw]

    return cx, cy, cyaw, ck

def get_forward_course(dl):
    ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
    ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=dl)

    return cx, cy, cyaw, ck

# get_switch_back_course生成折返路径
def get_switch_back_course(dl):
    """
    input param:
     - dl: 采样点间距;
    output:
     - [cx,cy,cyaw,ck]: [路径点坐标系(x,y), 偏航角度, 曲率]
    """
    # 生成第一段路径
    ax = [0.0, 30.0, 6.0, 20.0, 35.0]
    ay = [0.0, 0.0, 20.0, 35.0, 20.0]
    # [cx, cy, cyaw, ck, s]: [路径点坐标, 偏航角, 曲率, 长度]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=dl)

    # 生成第二段路径
    ax = [35.0, 10.0, 0.0, 0.0]
    ay = [20.0, 30.0, 5.0, 0.0]
    cx2, cy2, cyaw2, ck2, s2 = cubic_spline_planner.calc_spline_course(ax, ay, ds=dl)
    cyaw2 = [i - math.pi for i in cyaw2] # 反转第二段路径的方向

    # 合并两段路径
    cx.extend(cx2)
    cy.extend(cy2)
    cyaw.extend(cyaw2)
    ck.extend(ck2)

    return cx, cy, cyaw, ck

# 整个流程大概是：
# 1. 生成参考路径，
# 2. 设置初始状态，
# 3. 通过MPC计算控制输入（加速度和转向角），更新车辆状态，循环直到到达目标或超时。
# 4. MPC在每个时间步根据当前状态和参考轨迹，优化未来T个时间步的控制输入，
# 但只执行第一个控制输入，然后重新进行优化，属于滚动时域控制
def main():
    print(__file__ + " start!!")
    start = time.time()

    # 生成参考路径
    dl = 1.0  # course tick 采样点的间距
    #cx, cy, cyaw, ck = get_straight_course(dl) # 直线
    # cx, cy, cyaw, ck = get_straight_course2(dl) # 波浪线
    # cx, cy, cyaw, ck = get_straight_course3(dl) # 波浪线
    cx, cy, cyaw, ck = get_forward_course(dl) # 前进折返路线
    #cx, cy, cyaw, ck = get_switch_back_course(dl) # 折返路径, 数据格式:[x列表], [y列表], [偏航角列表], [曲率列表]
    print(f"[Info] line {sys._getframe().f_lineno}:", f"cx: {np.round(cx, 4)}")
    print(f"[Info] line {sys._getframe().f_lineno}:", f"cy: {np.round(cy, 4)}")
    print(f"[Info] line {sys._getframe().f_lineno}:", f"cyaw: {np.round(cyaw, 4)}")
    print(f"[Info] line {sys._getframe().f_lineno}:", f"ck: {np.round(ck, 4)}")

    # 生成路径点目标速度列表
    sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
    # sp = calc_speed_profile_v2(cx, cy, cyaw, ck, TARGET_SPEED)
    print(f"[Info] line {sys._getframe().f_lineno}:", f"sp: {np.round(sp, 4)}")

    # 设置车辆初始状态
    initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)

    # 仿真过程
    t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state)

    # 计算耗时
    elapsed_time = time.time() - start
    print(f"calc time:{elapsed_time:.6f} [sec]")

    if show_animation:  # pragma: no cover
        plt.close("all") # 关闭所有图形
        plt.subplots()   # 创建子图
        # 绘制路径跟踪图
        plt.plot(cx, cy, "-r", label="spline") # 绘制路径（spline）
        plt.plot(x, y, "-g", label="tracking") # 绘制跟踪轨迹（tracking）
        plt.grid(True)      # 设置网格
        plt.axis("equal")   # 设置坐标轴比例
        plt.xlabel("x[m]")  # 设置坐标轴标签
        plt.ylabel("y[m]")  # 
        plt.legend()        # 显示图例

        # 创建第二个子图
        plt.subplots()
        # 绘制速度图
        plt.plot(t, v, "-r", label="speed") # 绘制速度曲线
        plt.grid(True)              # 设置网格
        # 设置坐标轴标签
        plt.xlabel("Time [s]")
        plt.ylabel("Speed [kmh]")
        # 显示图形
        plt.show()


def main2():
    print(__file__ + " start!!")
    start = time.time()

    dl = 1.0  # course tick
    cx, cy, cyaw, ck = get_straight_course3(dl)

    sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)

    initial_state = State(x=cx[0], y=cy[0], yaw=0.0, v=0.0)

    t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state)

    elapsed_time = time.time() - start
    print(f"calc time:{elapsed_time:.6f} [sec]")

    if show_animation:  # pragma: no cover
        plt.close("all")
        plt.subplots()
        plt.plot(cx, cy, "-r", label="spline")
        plt.plot(x, y, "-g", label="tracking")
        plt.grid(True)
        plt.axis("equal")
        plt.xlabel("x[m]")
        plt.ylabel("y[m]")
        plt.legend()

        plt.subplots()
        plt.plot(t, v, "-r", label="speed")
        plt.grid(True)
        plt.xlabel("Time [s]")
        plt.ylabel("Speed [kmh]")

        plt.show()


if __name__ == '__main__':
    main()
    # main2()
